
#include "gnssimu-sample-v7.h"

namespace gnssimu_sample_v7
{
	class GnssImuSampleVars : public SessionSampleVars
	{
	public:
		SPADAS_VARS(GnssImuSample, SessionSampleVars)
		GnssImuSampleData data;

		Bool supportInterpolation() override
		{
			return TRUE;
		}

		SessionGeneralSample toGeneralSample()
		{
			SessionGeneralSample sample;
			sample.timestamp() = timestamp;

			Array<GeneralElement> values(40);
			sample.values() = values;

			values[0] = (Double)(Int)data.locationMode;
			values[1] = data.longitude;
			values[2] = data.latitude;
			values[3] = data.altitude;
			values[4] = data.speed;
			values[5] = data.orientation;
			values[6] = data.pitch;
			values[7] = data.roll;
			values[8] = data.yawRate;
			values[9] = data.ax;
			values[10] = data.ay;
			values[11] = data.az;
			values[12] = data.slipAngle;
			values[13] = data.jerkX;
			values[14] = data.jerkY;
			values[15] = data.satelliteCount.valid ? data.satelliteCount.value : OptionalDouble();
			values[16] = data.antennaLongitude;
			values[17] = data.antennaLatitude;
			values[18] = data.antennaAltitude;
			values[19] = (Double)(Int)data.gnssTimeType;
			values[20] = data.arrivalTime;
			values[21] = data.gnssTime.isValid() ? data.gnssTime.refValue().dateTime.year : OptionalDouble();
			values[22] = data.gnssTime.isValid() ? data.gnssTime.refValue().dateTime.month : OptionalDouble();
			values[23] = data.gnssTime.isValid() ? data.gnssTime.refValue().dateTime.day : OptionalDouble();
			values[24] = data.gnssTime.isValid() ? data.gnssTime.refValue().dateTime.hour : OptionalDouble();
			values[25] = data.gnssTime.isValid() ? data.gnssTime.refValue().dateTime.minute : OptionalDouble();
			values[26] = data.gnssTime.isValid() ? data.gnssTime.refValue().dateTime.second : OptionalDouble();
			values[27] = data.gnssTime.isValid() ? (Double)data.gnssTime.refValue().milliseconds : OptionalDouble();
			values[28] = data.pitchRate;
			values[29] = data.rollRate;
			values[30] = data.horizontalError;
			values[31] = data.verticalError;
			values[32] = data.curvature;

			sample.significantCount() = values.size();
			return sample;
		}

		Bool fromSample(String protocol, SessionSample sample) override
		{
			if (!sample.is<SessionGeneralSample>()) return FALSE;

			auto generalSample = sample.as<SessionGeneralSample>();
			timestamp = generalSample.timestamp();
			auto values = generalSample.values();

			if (protocol == "gnssimu-sample-v7")
			{
				if (values.size() != 40) return FALSE;

				data.locationMode = values[0].valid ? (GnssImuLocationMode)(Int)values[0].value : GnssImuLocationMode::NoLocation;
				data.longitude = values[1];
				data.latitude = values[2];
				data.altitude = values[3];
				data.speed = values[4];
				data.orientation = values[5];
				data.pitch = values[6];
				data.roll = values[7];
				data.yawRate = values[8];
				data.ax = values[9];
				data.ay = values[10];
				data.az = values[11];
				data.slipAngle = values[12];
				data.jerkX = values[13];
				data.jerkY = values[14];
				data.satelliteCount = values[15].valid ? (Int)values[15].value : OptionalInt();
				data.antennaLongitude = values[16];
				data.antennaLatitude = values[17];
				data.antennaAltitude = values[18];

				data.gnssTimeType = values[19].valid ? (GnssTimeType)(Int)values[19].value : GnssTimeType::UTC;
				data.arrivalTime = values[20];
				if (values[21].valid &&
					values[22].valid &&
					values[23].valid &&
					values[24].valid &&
					values[25].valid &&
					values[26].valid &&
					values[27].valid)
				{
					data.gnssTime = TimeWithMS((UInt)values[21].value,
						(UInt)values[22].value,
						(UInt)values[23].value,
						(UInt)values[24].value,
						(UInt)values[25].value,
						(UInt)values[26].value,
						(UInt)values[27].value);
				}
				else
				{
					data.gnssTime = Optional<TimeWithMS>();
				}

				data.pitchRate = values[28];
				data.rollRate = values[29];
				data.horizontalError = values[30];
				data.verticalError = values[31];
				data.curvature = values[32];

				return TRUE;
			}
			else if (protocol == "gnssimu-sample-v6")
			{
				if (values.size() != 28) return FALSE;

				data.locationMode = values[0].valid ? (GnssImuLocationMode)(Int)values[0].value : GnssImuLocationMode::NoLocation;
				data.longitude = values[1];
				data.latitude = values[2];
				data.altitude = values[3];
				data.speed = values[4];
				data.orientation = values[5];
				data.pitch = values[6];
				data.roll = values[7];
				data.yawRate = values[8];
				data.ax = values[9];
				data.ay = values[10];
				data.az = values[11];
				data.slipAngle = values[12];
				data.jerkX = values[13];
				data.jerkY = values[14];
				data.satelliteCount = values[15].valid ? (Int)values[15].value : OptionalInt();
				data.antennaLongitude = values[16];
				data.antennaLatitude = values[17];
				data.antennaAltitude = values[18];

				data.gnssTimeType = values[19].valid ? (GnssTimeType)(Int)values[19].value : GnssTimeType::UTC;
				data.arrivalTime = values[20];
				if (values[21].valid &&
					values[22].valid &&
					values[23].valid &&
					values[24].valid &&
					values[25].valid &&
					values[26].valid &&
					values[27].valid)
				{
					data.gnssTime = TimeWithMS((UInt)values[21].value,
						(UInt)values[22].value,
						(UInt)values[23].value,
						(UInt)values[24].value,
						(UInt)values[25].value,
						(UInt)values[26].value,
						(UInt)values[27].value);
				}
				else
				{
					data.gnssTime = Optional<TimeWithMS>();
				}

				return TRUE;
			}
			else if (protocol == "gnssimu-sample-v5")
			{
				if (values.size() != 19) return FALSE;

				data.locationMode = values[0].valid ? (GnssImuLocationMode)(Int)values[0].value : GnssImuLocationMode::NoLocation;
				data.longitude = values[1];
				data.latitude = values[2];
				data.antennaAltitude = values[3];
				data.speed = values[4];
				data.orientation = values[5];
				data.pitch = values[6];
				data.roll = values[7];
				data.yawRate = values[8];
				data.ax = values[9];
				data.ay = values[10];
				data.az = values[11];

				data.gnssTimeType = GnssTimeType::UTC;
				data.arrivalTime = timestamp.offset;
				if (values[12].valid &&
					values[13].valid &&
					values[14].valid &&
					values[15].valid &&
					values[16].valid &&
					values[17].valid &&
					values[18].valid)
				{
					data.gnssTime = TimeWithMS((UInt)values[12].value,
						(UInt)values[13].value,
						(UInt)values[14].value,
						(UInt)values[15].value,
						(UInt)values[16].value,
						(UInt)values[17].value,
						(UInt)values[18].value);
				}
				else
				{
					data.gnssTime = Optional<TimeWithMS>();
				}

				return TRUE;
			}
			else if (protocol == "gnssimu-sample-v4")
			{
				if (values.size() != 18) return FALSE;

				data.longitude = values[0];
				data.latitude = values[1];
				data.antennaAltitude = values[2];
				data.speed = values[3];
				data.orientation = values[4];
				data.pitch = values[5];
				data.roll = values[6];
				data.yawRate = values[7];
				data.ax = values[8];
				data.ay = values[9];
				data.az = values[10];

				data.locationMode = data.longitude.valid && data.latitude.valid ? GnssImuLocationMode::NoLocation : GnssImuLocationMode::NoLocation;

				data.gnssTimeType = GnssTimeType::UTC;
				data.arrivalTime = timestamp.offset;
				if (values[11].valid &&
					values[12].valid &&
					values[13].valid &&
					values[14].valid &&
					values[15].valid &&
					values[16].valid &&
					values[17].valid)
				{
					data.gnssTime = TimeWithMS((UInt)values[11].value,
						(UInt)values[12].value,
						(UInt)values[13].value,
						(UInt)values[14].value,
						(UInt)values[15].value,
						(UInt)values[16].value,
						(UInt)values[17].value);
				}
				else
				{
					data.gnssTime = Optional<TimeWithMS>();
				}

				return TRUE;
			}

			return FALSE;
		}

		SessionSample interpolate(SessionSample& ss1, Double w1, SessionSample& ss2, Double w2, FullTimestamp timestamp) override
		{
			if (!ss1.is<GnssImuSample>() || !ss2.is<GnssImuSample>()) return SessionSample();

			GnssImuSampleData& s1 = ss1.as<GnssImuSample>().getVars()->data;
			GnssImuSampleData& s2 = ss2.as<GnssImuSample>().getVars()->data;

			GnssImuSample gnssImuSample;
			gnssImuSample.timestamp() = timestamp;
			
			GnssImuSampleData& output = gnssImuSample.getVars()->data;
            if (s1.locationMode == s2.locationMode)
            {
                output.locationMode = s1.locationMode;
                output.longitude = s1.longitude * w1 + s2.longitude * w2;
                output.latitude = s1.latitude * w1 + s2.latitude * w2;
                output.altitude = s1.altitude * w1 + s2.altitude * w2;
                output.antennaLongitude = s1.antennaLongitude * w1 + s2.antennaLongitude * w2;
                output.antennaLatitude = s1.antennaLatitude * w1 + s2.antennaLatitude * w2;
                output.antennaAltitude = s1.antennaAltitude * w1 + s2.antennaAltitude * w2;
            }
            else
            {
                output.locationMode = w1 > w2 ? s1.locationMode : s2.locationMode;
                output.longitude = w1 > w2 ? s1.longitude : s2.longitude;
                output.latitude = w1 > w2 ? s1.latitude : s2.latitude;
                output.altitude = w1 > w2 ? s1.altitude : s2.altitude;
                output.antennaLongitude = w1 > w2 ? s1.antennaLongitude : s2.antennaLongitude;
                output.antennaLatitude = w1 > w2 ? s1.antennaLatitude : s2.antennaLatitude;
                output.antennaAltitude = w1 > w2 ? s1.antennaAltitude : s2.antennaAltitude;
            }

            output.satelliteCount = w1 > w2 ? s1.satelliteCount : s2.satelliteCount;

            output.speed = s1.speed * w1 + s2.speed * w2;
            output.orientation = interpolateAngle(s1.orientation, w1, s2.orientation, w2);
            output.pitch = s1.pitch * w1 + s2.pitch * w2;
            output.roll = s1.roll * w1 + s2.roll * w2;
            output.yawRate = s1.yawRate * w1 + s2.yawRate * w2;
            output.ax = s1.ax * w1 + s2.ax * w2;
            output.ay = s1.ay * w1 + s2.ay * w2;
            output.az = s1.az * w1 + s2.az * w2;
            output.slipAngle = s1.slipAngle * w1 + s2.slipAngle * w2;
            output.jerkX = s1.jerkX * w1 + s2.jerkX * w2;
            output.jerkY = s1.jerkY * w1 + s2.jerkY * w2;

            output.gnssTimeType = s1.gnssTimeType;
            output.arrivalTime = s1.arrivalTime * w1 + s2.arrivalTime * w2;
            if (s1.gnssTime.isValid() && s2.gnssTime.isValid())
            {
                Long diffMS = s2.gnssTime.value() - s1.gnssTime.value();
                output.gnssTime = s1.gnssTime.value() + (Long)(diffMS * w2 / (w1 + w2));
            }

            output.pitchRate = s1.pitchRate * w1 + s2.pitchRate * w2;
            output.rollRate = s1.rollRate * w1 + s2.rollRate * w2;
            output.horizontalError = s1.horizontalError * w1 + s2.horizontalError * w2;
            output.verticalError = s1.verticalError * w1 + s2.verticalError * w2;
            output.curvature = s1.curvature * w1 + s2.curvature * w2;

			return gnssImuSample.as<SessionSample>();
		}

        OptionalDouble interpolateAngle(OptionalDouble a1, Double w1, OptionalDouble a2, Double w2)
        {
            if (!a1.valid || !a2.valid) return OptionalDouble();

            Double x1 = math::cos(a1.value);
            Double y1 = math::sin(a1.value);
            Double x2 = math::cos(a2.value);
            Double y2 = math::sin(a2.value);

            Double xo = x1 * w1 + x2 * w2;
            Double yo = y1 * w1 + y2 * w2;
            if (xo == 0 && yo == 0) return OptionalDouble();

            return math::atan(yo, xo);
        }
	};
}

using namespace gnssimu_sample_v7;

GnssImuSample::GnssImuSample() : Object<GnssImuSampleVars>(new GnssImuSampleVars, TRUE)
{}

FullTimestamp& GnssImuSample::timestamp()
{
	return vars->timestamp;
}

GnssImuSampleData *GnssImuSample::operator ->()
{
    return &vars->data;
}

String GnssImuSample::getProtocolName(UInt channel)
{
    return (String)"gnssimu-sample-v7@" + channel;
}

SessionGeneralSample GnssImuSample::toGeneralSample()
{
    return vars->toGeneralSample();
}

Array<String> GnssImuSample::getProtocolNames(UInt channel)
{
	return Array<String>::create(4, "gnssimu-sample-v4", "gnssimu-sample-v5", "gnssimu-sample-v6", "gnssimu-sample-v7").convert<String>([channel](auto& s){ return s + "@" + channel; });
}

Bool GnssImuSample::fromGeneralSample(String protocol, SessionGeneralSample generalSample)
{
    return vars->fromSample(protocol, generalSample.as<SessionSample>());
}